#include "StdAfx.h"
#include "Analyzer.h"


Analyzer::Analyzer(World* basicWorld, const btVector3& minAABB, const btVector3& maxAABB, btScalar stepSize) 
{

	this->world = basicWorld->GetWorld();
	btCollisionObjectArray tmp = this->world->getCollisionObjectArray();
	std::cout << "Il. obiektow kolizyjnych: " << this->collisionObjects.size() << std::endl;
	for(int i = 0; i < tmp.size(); ++i)
	{
		collisionObjects.push_back(tmp[i]);
		std::cout << "\t" << collisionObjects[i] << std::endl;
	}

	this->minAABB = minAABB;
	this->maxAABB = maxAABB;

	//Triangle::Print(this->minAABB);
	std::cout << std::endl;
	//Triangle::Print(this->maxAABB);
	std::cout << std::endl;

	this->stepSize = stepSize;

	btVector3 worldSize = this->maxAABB - this->minAABB;
	this->maxX = worldSize.getX() / stepSize;
	this->maxY = worldSize.getY() / stepSize;
	this->maxZ = worldSize.getZ() / stepSize;

	this->InitMap(maxX, maxY, maxZ);
	std::cout << "done." << std::endl;

	float dx = stepSize / 2.0f - stepSize * 0.2f;

	btVector3 tmpVector = btVector3(stepSize/2.0f - dx, stepSize/2.0f - dx, stepSize/2.0f - dx);
	this->testCube = basicWorld->CreateBox(tmpVector, 0);
	world->addCollisionObject(this->testCube, 1, 1 | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
}


Analyzer::~Analyzer(void)
{

}


void Analyzer::Create3DRaster()
{
	std::cout << "Start analyze..." << std::endl;

	btTransform transform = btTransform::getIdentity();

	for(unsigned int x = 0; x < maxX; ++x)
	{
		for(unsigned int y = 0; y < maxY; ++y)
		{
			for(unsigned int z = 0; z < maxZ; ++z)
			{
				transform.setIdentity();

				float tmpX = x*stepSize - std::abs(minAABB.getX());
				float tmpY = y*stepSize - std::abs(minAABB.getY());
				float tmpZ = z*stepSize - std::abs(minAABB.getZ());

				//std::cout << tmpX << " " << tmpY << " " << tmpZ << std::endl;
				/*std::cout << "\tx:" << x  << "\t" << (float)x * stepSize << std::endl;
				std::cout << "\ty:" << y  << "\t" << (float)y * stepSize << std::endl;
				std::cout << "\tz:" << z  << "\t" << (float)z * stepSize << std::endl;*/
				btVector3 p = btVector3(tmpX, tmpY, tmpZ);
				transform.setOrigin(p);

				this->testCube->setWorldTransform(transform);
				//std::cout << x << " " << y << " " << z << ": " << (map[x][y][z].Occupied ? "1" : "0") << "\t" << map[x][y][z].Obstacles.size() << std::endl;
				map[x][y][z].Occupied = this->CheckCollision(map[x][y][z].Obstacles);							
				/*if(map[x][y][z].Obstacles.size())
					std::cout << x << " " << y << " " << z << ": " << map[x][y][z].Obstacles.size() << std::endl;*/
				//std::cout << x << " " << y << " " << z << ": " << (map[x][y][z].Occupied ? "1" : "0") << "\t" << map[x][y][z].Obstacles.size() << std::endl << std::endl;
			}
		}
	}  

	this->world->removeCollisionObject(this->testCube);

	delete this->testCube;
	this->testCube = nullptr;
}


bool Analyzer::CheckCollision(std::set<int>& obstacles)
{
	world->performDiscreteCollisionDetection();

	int num = world->getDispatcher()->getNumManifolds();
	bool result = false;
	for(int k = 0; k < num; ++k)
	{

		btPersistentManifold* contactMainfolds = world->getDispatcher()->getManifoldByIndexInternal(k);

		btRigidBody* A = static_cast<btRigidBody*>(contactMainfolds->getBody0());
		btRigidBody* B = static_cast<btRigidBody*>(contactMainfolds->getBody1());

		if(A != this->testCube && B != this->testCube)
			continue;

		int n = contactMainfolds->getNumContacts();

		if(n > 0)
		{

			btTransform t1 = A->getWorldTransform();
			btTransform t2 = B->getWorldTransform();

			/*std::cout << "( " << t1.getOrigin().getX() << ", " << t1.getOrigin().getY() << ", " << t1.getOrigin().getZ() << " )\t( " << t2.getOrigin().getX() << ", " << t2.getOrigin().getY() << ", " << t2.getOrigin().getZ() << " )" << std::endl;
			for(int i = 0; i < n; ++i)
			{
			btManifoldPoint p = contactMainfolds->getContactPoint(i);
			float distance = p.getDistance();
			bool test = contactMainfolds->validContactDistance(p);
			btVector3 posA = p.getPositionWorldOnA();
			btVector3 posB = p.getPositionWorldOnB();

			std::cout << "\t(" << posA.getX() << ", " << posA.getY() << ", " << posA.getZ() << ") ( " << posB.getX() << ", " << posB.getY() << ", " << posB.getZ() << ") " << distance << " " << test << std::endl;

			}
			std::cout << std::endl;*/
			int index;
			if(A == this->testCube)
			{
				btCollisionObject* tmp = dynamic_cast<btCollisionObject*>(B);
				//std::cout << tmp << std::endl;
				index = this->collisionObjects.findLinearSearch(tmp);

				if(index !=	this->collisionObjects.size())
					obstacles.insert(index);
				//std::cout << "\t" << index << " " << obstacles.size() << std::endl;
			}
			else
			{
				btCollisionObject* tmp = dynamic_cast<btCollisionObject*>(A);
				//std::cout << tmp << std::endl;
				index = this->collisionObjects.findLinearSearch(tmp);

				if(index != this->collisionObjects.size())
					obstacles.insert(index);
				//std::cout << "\t2.Indeks: " << index << " Rozmiar: " << obstacles.size() << "\t" << this->collisionObjects.size() << std::endl;				
			}

			result = true;
		}    		
		world->getDispatcher()->clearManifold(contactMainfolds);
	}

	return result;
}

void Analyzer::InitMap(int x, int y, int z)
{
	/*x+=1;
	y+=1;
	z+=1;*/
	std::cout << std::endl <<  x << " " << y << " " << z << std::endl;

#if _DEBUG
	Raster3D tmp(boost::extents[x][y][z]);
	memcpy(&this->map, &tmp, sizeof(Raster3D));
#else
	this->map.resize(boost::extents[x][y][z]);
#endif

	std::cout << this->map.shape()[0] << " " << this->map.shape()[1] << " " << this->map.shape()[2] << std::endl;
	//this->map.resize(boost::extents[x][y][z]);
	this->size_x = x;
	this->size_y = y;
	this->size_z = z;	
}

//bool Analyzer::PointToHigh(btVector3& p)
//{
//	btVector3 target = btVector3(p.getX(), this->minAABB.getY(), p.getZ());
//	btCollisionWorld::ClosestRayResultCallback  result(p, target);
//	this->world->rayTest(p, target, result);
//	
//	if(result.hasHit())
//	{
//		btVector3 hitPoint = result.m_hitPointWorld;
//		if(p.distance(hitPoint) / stepSize <= 2.0f)
//			return true;
//	}
//	else
//	{
//		if(p.distance(target) / stepSize > 2.0f)
//			return true;
//	}
//
//	return false;
//}

void Analyzer::Clamp(PointInfo& p)
{
	//std::cout << "\t\t\tClamp: " << p.ToString() << " in AABB (" << minAABB.getX() << ", " << minAABB.getY() << ", " << minAABB.getZ() << ") (" << maxAABB.getX() << ", " << maxAABB.getY() << ", " << maxAABB.getZ() << ") to values ";
	if(p.X * stepSize < minAABB.getX())
		p.X = minAABB.getX();
	else if(p.X * stepSize  >= maxAABB.getX())
		p.X = maxAABB.getX() - 1;

	if(p.Y * stepSize  < minAABB.getY())
		p.Y = minAABB.getY();
	else if(p.Y * stepSize  >= maxAABB.getY())
		p.Y = maxAABB.getY() - 1;

	if(p.Z * stepSize  < minAABB.getZ())
		p.Z = minAABB.getZ();
	else if(p.Z * stepSize  >= maxAABB.getZ())
		p.Z = maxAABB.getZ() - 1;

	//std::cout << p.ToString() << std::endl;
}

bool Analyzer::IsNearObstacle(const PointInfo& p, btAlignedObjectArray<int>& obstaclesIndex)
{
	PointInfo start, end;
	start.X = p.X - 1;
	start.Y = p.Y - 1;
	start.Z = p.Z - 1;

	end.X = p.X + 1;
	end.Y = p.Y + 1;
	end.Z = p.Z + 1;

	this->Clamp(start);
	this->Clamp(end);
	//std::cout << "\t\tStart/End" << start.ToString() << "\t" << end.ToString() << std::endl;

	bool result = false;

	for(int i = start.X; i <= end.X; ++i)
	{
		for(int j = start.Y; j <= end.Y; ++j)
		{
			for(int k = start.Z; k <= end.Z; ++k)
			{
				if(i == p.X && j == p.Y && k == p.Z)
					continue;

				if(map[i][j][k].Occupied)
				{
					result = true;
					//std::cout << i << " " << j << " " << k << ": " << map[i][j][k].Obstacles.size() << std::endl;
					for(std::set<int>::iterator iter = map[i][j][k].Obstacles.begin(); iter != map[i][j][k].Obstacles.end(); ++iter)
					{
						//std::cout << "\t" << *iter << std::endl;
						int test = obstaclesIndex.findBinarySearch(*iter);
						//std::cout << "\t\tSzukam klucza: " << *iter << " Jego pozycja: " << test << " " << obstaclesIndex.size() << std::endl;
						if(test == obstaclesIndex.size())
							obstaclesIndex.push_back(*iter);
					}					
				}
			}
		}
	}
	obstaclesIndex.size();
	return result;
}

int Analyzer::ComputeNeighborhood(PointInfo& p)
{
	static int MAX = -1;
	if(this->map[p.X][p.Y][p.Z].Occupied)
		return -1;

	PointInfo start, end;
	start.X = p.X - 1;
	start.Y = p.Y - 1;
	start.Z = p.Z - 1;

	end.X = p.X + 1;
	end.Y = p.Y + 1;
	end.Z = p.Z + 1;

	this->Clamp(start);
	this->Clamp(end);

	//std::cout << end.X << "\t" << end.Y << "\t" << end.Z << std::endl;
	int sum = 0;



	for(int i = start.X; i <= end.X; ++i)
	{
		for(int j = start.Y; j <= end.Y; ++j)
		{
			for(int k = start.Z; k <= end.Z; ++k)
			{
				if(i == p.X && j == p.Y && k == p.Z)
					continue;
				if(!this->map[i][j][k].Occupied)
					++sum;				
			}
		}
	}
	if(MAX < sum) 
	{
		MAX = sum;
		std::cout << sum << std::endl;
	}
	return (sum == MAX_WEIGHT ? 0 : sum);
}

void Analyzer::MarkNavPoints()
{	
	std::cout << "Nav points" << std::endl;
	for(unsigned int x = 0; x < maxX; ++x)
	{
		for(unsigned int y = 0; y < maxY; ++y)
		{
			for(unsigned int z = 0; z < maxZ; ++z)
			{
				PointInfo p;
				p.X = x;
				p.Y = y;
				p.Z = z;
				p.RealCoords.X = this->minAABB.x() + x * stepSize;
				p.RealCoords.Y = this->minAABB.y() + y * stepSize;
				p.RealCoords.Z = this->minAABB.z() + z * stepSize;

				//std::cout << p.RealCoords.ToString() << std::endl;

				int weight = this->ComputeNeighborhood(p);

				if(weight > 0)
				{
					navPoints.push_back(p);
				}
			}
		}
	}

	std::cout << "Nav points count: " << navPoints.size() << std::endl;
	//for(unsigned int i = 0; i < navPoints.size(); ++i)
	//{
	//	this->ComputeNormal(navPoints[i]);		
	//	//std::cout << navPoints[i].NX << "\t"  << navPoints[i].NY << "\t"  << navPoints[i].NZ << std::endl << std::endl;
	//	map[navPoints[i].X][navPoints[i].Y][navPoints[i].Z].NX = navPoints[i].NX;
	//	map[navPoints[i].X][navPoints[i].Y][navPoints[i].Z].NY = navPoints[i].NY;
	//	map[navPoints[i].X][navPoints[i].Y][navPoints[i].Z].NZ = navPoints[i].NZ;
	//}
}

void Analyzer::Run()
{
	this->Create3DRaster();

	//this->PrintResult1();
	this->MarkNavPoints();


	//std::cout << "=====================" << std::endl;
	//for(int i = 0; i < size_x; ++i)
	//{
	//	for(int j = 0; j < size_y; ++j)
	//	{
	//		for(int k = 0; k < size_z; ++k)
	//		{
	//			//std::cout << "\t\t(" << map[i][j][k].NX << ","<< map[i][j][k].NY << "," << map[i][j][k].NZ << ")";
	//			std::cout << "\t" << (map[i][j][k].Occupied  ? "0" : "1");//(map[i][j][k].Occupied ? "1" : "0");
	//			
	//		}
	//		std::cout << std::endl;
	//	}
	//	std::cout << std::endl;
	//}

	this->BuildGraphs();
	std::cout << std::endl << std::endl;
	int n = navPoints.size();
	/*
	for(int i = 0; i < n; ++i)
	std::cout << i << ".\t" << navPoints[i].X << "\t" << navPoints[i].Y << "\t" << navPoints[i].Z << std::endl;
	std::cout << std::endl << std::endl;*/



	//this->RemoveCollinearPoints();

	/*for(int i = 0; i < size_x; ++i)
	{
		for(int j = 0; j < size_y; ++j)
		{
			for(int k = 0; k < size_z; ++k)
			{

				std::cout << "\t" << (map[i][j][k].Visible ? "1" : "0");

			}
			std::cout << std::endl;
		}
		std::cout << std::endl;
	}*/


	std::cout << std::endl << std::endl;
}

void Analyzer::BuildGraphs()
{
	std::cout << "Buduje grafy..." << std::endl;
	int n = navPoints.size();
	std::set<int> keys;
	for(int i = 0; i < n; ++i)
	{
		btAlignedObjectArray<int> obstacles;
		//std::cout << "\t" << navPoints[i].ToString() << "\t" << navPoints[i].RealCoords.ToString() << std::endl;
		if(this->IsNearObstacle(navPoints[i], obstacles))
		{
			//std::cout << navPoints[i].ToString() << " is near obstacles\t" << obstacles.size() << std::endl;
			for(int j = 0; j < obstacles.size(); ++j)
			{
				this->hash.Insert(obstacles[j], navPoints[i]);
				//std::cout << "\tDodaje klucz: " << obstacles[j] << std::endl;
				keys.insert(obstacles[j]);
			}

		}
		//else
		//{
		//	//std::cout << navPoints[i].ToString() << " is NOT near obstacles\t" << obstacles.size() << std::endl;
		//	//std::cout << "\tDodaje klucz: -1" << std::endl;
		//	this->hash.Insert(-1, navPoints[i]);
		//	keys.insert(-1);
		//}
	}
	DecompositionDesc desc;


	std::cout << "Keys: " << keys.size() << std::endl;
	btConvexHullComputer hullComputer;
	//btAlignedObjectArray<btConvexHullComputer> hullComputer;
	/*for(int i = 0; i < keys.size(); ++i)
		hullComputer.push_back(btConvexHullComputer());*/

	int index = 0;
	for(std::set<int>::iterator it = keys.begin(); it != keys.end(); ++it)
	{
		int tmp = *it;
		std::cout << tmp;
		btAlignedObjectArray<PointInfo> tmpArray = hash.GetArray(tmp);
		std::cout << "ArraySize: " << tmpArray.size() << std::endl;
		btAlignedObjectArray<btVector3> real;

		for(int i = 0; i < tmpArray.size(); ++i)
		{
			real.push_back(btVector3(tmpArray[i].RealCoords.X, tmpArray[i].RealCoords.Y, tmpArray[i].RealCoords.Z));
			/*std::cout << "\t";
			Triangle::Print(real[i]);
			std::cout << "\t" << tmpArray[i].RealCoords.ToString();
			std::cout << std::endl;*/
			
		}

		try
		{
			/*btAlignedObjectArray<PointInfo>& result = GraphManager::Reduce(this->world, tmpArray);

			for(int i = 0; i < result.size(); ++i)
			{
				std::cout << "\t\tAdding: " << result[i].RealCoords.ToString() << std::endl;
				desc.mVertices.push_back(btVector3( result[i].RealCoords.X, result[i].RealCoords.Y, result[i].RealCoords.Z));
			}*/

			//btConvexHullComputer* hullComputer = new btConvexHullComputer();
			
			ConvexHull* hullComputer = new ConvexHull();
			hullComputer->vertices.clear();
			hullComputer->compute(&(real[0].getX()), sizeof(btVector3), real.size(), 0.f, 0.f);
			hullComputer->CorrectPrecision();
			std::cout << "Points:" << std::endl;
			for(int i = 0; i < hullComputer->vertices.size(); ++i)
			{
				std::cout << "\t";
				//Triangle::Print(hullComputer->vertices[i]);
				std::cout << std::endl;
			}
			desc.mConvexHulls.push_back(hullComputer);
			std::cout << "Hull: " << hullComputer->vertices.size() << std::endl;
			hullComputer->ComputePlanes();
			/*hullComputer.edges.clear();
			hullComputer.faces.clear();
			hullComputer.vertices.clear();*/
			//delete hullComputer;
			//hullComputer = 0;
			////MeshBuilder::BuildMesh(result, this->world, this->collisionObjects[tmp], this->minAABB, this->maxAABB);
			//for(int i = 0; i < result.size(); ++i)
			//{
			//	//std::cout << result[i].ToString() << " " << result[i].Removed;
			//	map[result[i].X][result[i].Y][result[i].Z].Visible |= !result[i].Removed;
			//	//std::cout << (map[result[i].X][result[i].Y][result[i].Z].Visible ? "1" : "0") << std::endl;
			//}
		}catch(...)
		{
			std::cout << "Wyjatek: " << *it << std::endl;
		}		
	}

	

	desc.mMaxAABB = this->maxAABB;
	desc.mMinAABB = this->minAABB;
	desc.mWorld = this->world;
	desc.mStepSize = btVector3(this->stepSize,this->stepSize,this->stepSize);

	Decomposition decomp(desc);
	decomp.Decomp2(DecomposedHulls, ObstacleHulls);

	WorldHull = decomp.mWorldHull;
	/*for(int i = 0; i < decomp.mHulls.size(); ++i)
		ObstacleHulls.push_back(decomp.mHulls[i]);*/
	/*DecomposedHulls.clear();
	DecomposedHulls.reserve(decomp.OutPolygons.size());

	std::cout << decomp.OutPolygons.size() << std::endl;;

	for(int i = 0; i < decomp.OutPolygons.size(); ++i)
		DecomposedHulls.push_back(&(decomp.OutPolygons[i]));

	std::cout << DecomposedHulls.size() << std::endl;
	for(int i = 0; i < DecomposedHulls.size(); ++i)
	{
		std::cout << "Decomposed " << i << std::endl;
		for(int j = 0; j < DecomposedHulls[i]->vertices.size(); ++j)
		{
			std::cout << "\t";
			Triangle::Print(DecomposedHulls[i]->vertices[j]);
			std::cout << std::endl;
		}
		std::cout << std::endl;
	}*/

	std::cout << "Koniec dekompozycji"  << std::endl << std::endl;
}

//bool Analyzer::AreColinear(const PointInfo& p1, const PointInfo& p2, const PointInfo& p3)
//{
//	btVector3 A( p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z);
//	btVector3 B( p3.X - p1.X, p3.Y - p1.Y, p3.Z - p1.Z);
//
//	btVector3 cross = A.cross(B);
//
//	if(cross.length2() == 0)
//	{
//		return true;
//	}
//
//	return false;
//}

//
//void Analyzer::RemovePointFromNavMap(int i)
//{
//	const PointInfo& p = navPoints[i];
//	
//	//map[p.X][p.Y][p.Z].Removed = true;
//}
//
//void Analyzer::RemoveCollinearPoints()
//{
//	int n = navPoints.size();
//	int sum = 0;
//	for(int i = 0; i < n; ++i)
//	{
//		for(int j = i + 1; j < n; ++j)
//		{
//			for(int k = j + 1; k < n; ++k)
//			{
//				++sum;
//				if(graph[i][j] > 0 && graph[j][k] > 0 && graph[k][i] > 0)
//				{
//					
//					const PointInfo& p1 = navPoints[i];
//					const PointInfo& p2 = navPoints[j];
//					const PointInfo& p3 = navPoints[k];
//
//					if(Analyzer::AreColinear(p1, p2, p3))
//					{
//						/*bool obstacle1 = p1.NearObstacle;
//						bool obstacle2 = p2.NearObstacle;
//						bool obstacle3 = p3.NearObstacle;*/
//
//						bool allowToRemove = true;
//
//						if(!allowToRemove)
//							continue;
//
//						std::cout << "Colinear: ";
//						std::cout << p1.ToString() << "\t";
//						std::cout << p2.ToString() << "\t";
//						std::cout << p3.ToString() << std::endl;
//						std::cout << "\t\t" << i << "\t" << j << "\t" << k << std::endl;
//						btVector3* v1 = new btVector3(p1.X, p1.Y, p1.Z);
//						btVector3* v2 = new btVector3(p2.X, p2.Y, p2.Z);
//						btVector3* v3 = new btVector3(p3.X, p3.Y, p3.Z);
//
//						bool removed = false;
//
//						
//
//	/*					std::cout << "\t\tObstacle1:\t" << obstacle1 << std::endl;
//						std::cout << "\t\tObstacle2:\t" << obstacle2 << std::endl;
//						std::cout << "\t\tObstacle3:\t" << obstacle3 << std::endl;*/
//						std::cout << "\t\tAllow:\t" << allowToRemove << std::endl;
//
//						if(!removed)
//						{
//							btVector3* v12 = new btVector3((*v2 - *v1));
//							btVector3* v13 = new btVector3((*v3 - *v1));
//							btScalar angle = abs(v12->angle(*v13));
//							/*std::cout << "\t 12 vs 13: " << angle << std::endl;*/
//							if(angle > 0)
//							{
//								removed = true;
//								this->RemovePointFromNavMap(i);
//								std::cout << "\t\tRemoved " << 1 << std::endl;
//							}
//
//							delete v12;
//							delete v13;
//						}
//
//						if(!removed)
//						{
//							btVector3* v21 = new btVector3((*v1 - *v2));
//							btVector3* v23 = new btVector3((*v3 - *v2));
//							btScalar angle = abs(v21->angle(*v23));
//							//std::cout << "\t 21 vs 23: " << angle << std::endl;
//							if(angle > 0)
//							{
//								removed = true;
//								this->RemovePointFromNavMap(j);
//								std::cout << "\t\tRemoved " << 2 << std::endl;
//							}
//
//							delete v21;
//							delete v23;
//						}
//
//						if(!removed)
//						{
//							
//							btVector3* v31 = new btVector3((*v1 - *v3));
//							btVector3* v32 = new btVector3((*v2 - *v3));
//							btScalar angle = abs(v31->angle(*v32));
//							//std::cout << "\t 31 vs 32: " << angle << std::endl;
//							if(angle > 0)
//							{
//								removed = true;
//								this->RemovePointFromNavMap(k);
//								std::cout << "\t\tRemoved " << 3 << std::endl;
//							}
//
//							delete v31;
//							delete v32;
//						}						
//
//						delete v1;
//						delete v2;
//						delete v3;
//					}					
//				}
//			}
//		}
//	}
//
//	std::cout << n << std::endl;
//	std::cout << sum << std::endl;
//}